#include <math.h>
#include <unistd.h>

#include "lcm/lcm.h"

#include "common/getopt.h"
#include "common/timestamp.h"
#include "lcmtypes/diff_drive_t.h"

/* Example diff drive progrem.
 *
 * This program periodically outputs a diff_drive_t, containing
 * desired left and right motor ratios (each in [-1, 1]).
 */

int main(int argc, char *argv[])
{
    getopt_t *gopt = getopt_create();
    getopt_add_string(gopt, 'c', "channel", "DIFF_DRIVE", "LCM publish channel");

    if (!getopt_parse(gopt, argc, argv, 1)) {
        printf("Options:\n");
        getopt_do_usage(gopt);
        return 0;
    }

    char *channel = getopt_get_string(gopt, "channel");
    printf("Publishing on channel: %s\n", channel);

    lcm_t *lcm = lcm_create(NULL);
    diff_drive_t cmd = { .left = 0.0, .right = 0.0, .left_enabled = 0, .right_enabled = 0  };

    int64_t utime_start = timestamp_now();

    float MAX_SPEED = 0.5;

    while(1) {
        usleep(100000);
        cmd.utime = timestamp_now();
        double dt_s = (cmd.utime - utime_start) / 1000000.0;
        cmd.left  = MAX_SPEED * sin((dt_s - 0.5) / 2);
        cmd.right = MAX_SPEED * sin((dt_s - 0)   / 2);

        diff_drive_t_publish(lcm, channel, &cmd);
    }

    lcm_destroy(lcm);
    getopt_destroy(gopt);
    free(channel);
}
